% implement a basic particle filter sampling method 

% 书中的例5.1


close all; rng(10); clear all;
%参数
particleNumber  = 100;
simuTime         =50;

wSigma   =5; 
vSigma   =1;



% 初始
xlast =  0.1 ;
plast = 2;

particlesLast = xlast *(3*(rand(particleNumber,1)-0.5));
xestimates  = zeros(simuTime+1,1);

xestimates(1) =xlast; 

%生成测量序列
xtrue= xlast;

xtrues= zeros(simuTime+1,1);
xtrues(1) = xtrue;

ys    = zeros(simuTime+1,1);
ys(1) = 1/20*xtrue^2 +vSigma* randn;
for i =2:simuTime+1
    xlastTemp   =  xtrues(i-1);
    xtrues(i) = 1/2* xlastTemp + (25*xlastTemp)/(1+xlastTemp^2)+8*cos(1.2*(i)) + wSigma*randn;
    ys(i)     = 1/20*xtrues(i)^2  + vSigma*randn;
end

%%%粒子滤波

particlesPrediction =  zeros(particleNumber,1);
qs                   = zeros(particleNumber,1);
particlesRecord     =zeros(simuTime+1,particleNumber);
particlesRecord(1,:)  = particlesLast';
for i =2:simuTime+1
    particlesPrediction  =  1/2* particlesLast+ (25*particlesLast)./(1+particlesLast.^2) + ones(particleNumber,1)*8*cos(1.2*(i)) + wSigma*randn(particleNumber,1);
    for j =1: particleNumber
        qs(j)  = exp(loglikepdf ((ys(i)-1/20* particlesPrediction(j)^2)/vSigma));
    end

    %重采样，使得particle的分布与qs相同
    qs =  qs  /sum(qs);

    ind  = resampleSystematic(qs, particleNumber);

    particlesLast  = particlesPrediction(ind);

    particlesRecord(i,:)  = particlesLast';
end

xestimates  = mean(particlesRecord,2);

plot(xestimates);
hold on 
plot(xtrues,'c');
